/*
 * Omnidrive.h
 *
 *  Created on: Oct 31, 2010
 *      Author: eweber
 */

#ifndef OMNIDRIVE_H_
#define OMNIDRIVE_H_

#include "Base.h"
#include <stdlib.h>
#include <vxWorks.h>

class SpeedController;
class GenericHID;

class Omnidrive{
private:
	float mHeading;
	float mFacing;
	float mPower;

	float mSpeedFL;
	float mSpeedFR;
	float mSpeedRL;
	float mSpeedRR;

	SpeedController *mFrontLeft;
	SpeedController *mFrontRight;
	SpeedController *mRearLeft;
	SpeedController *mRearRight;

	void CalcMotorPowerAuton(void);
	void CalcMotorPowerTeleop(void);

public:
	Omnidrive(SpeedController, SpeedController, SpeedController, SpeedController);
	Omnidrive(unsigned char, unsigned char, unsigned char, unsigned char);
	Omnidrive();
	~Omnidrive();
	void SetPower(float);
	float GetPower(float);
	void SetFacing(float);
	float GetFacing(float);
	void SetHeading(float);
	float GetHeading(float);
	void UpdateMotorsAuton(void);
	void UpdateMotorsTeleop(void);

};



#endif /* OMNIDRIVE_H_ */
